package com.grt192.mechanism.cannonbot;

import com.grt192.actuator.GRTJaguar;
import com.grt192.controller.pid.AsynchronousPIDController;
import com.grt192.core.Mechanism;
import com.grt192.event.component.GyroEvent;
import com.grt192.event.component.GyroListener;
import com.grt192.event.component.EncoderEvent;
import com.grt192.event.component.EncoderListener;
import com.grt192.event.controller.PIDEvent;
import com.grt192.event.controller.PIDListener;
import com.grt192.sensor.GRTEncoder;
import com.grt192.sensor.GRTGyro;
import com.grt192.sensor.canjaguar.GRTJagEncoder;

import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.PIDOutput;

/**
 *
 * @author anand
 *
 */
public class CBPWMTankDriveTrain extends Mechanism implements PIDOutput,
        EncoderListener, GyroListener, PIDListener {

    public static final int WAIT_INTERVAL = 2;
    public static final int ROBOT_WIDTH = 28; // TODO find
    private double turnP = .10;
    private double turnI = .10;
    private double turnD = .10;
    private double driveP = .10;
    private double driveI = .10;
    private double driveD = .10;
    private GRTJaguar leftJaguar;
    private GRTJaguar rightJaguar;
    private GRTEncoder leftEncoder;
    private GRTEncoder rightEncoder;
    private GRTGyro canGyro;
    private double leftWheelX;
    private double leftWheelY;
    private double rightWheelX;
    private double rightWheelY;
    private boolean pointTurn;
    private AsynchronousPIDController turnControl;
    private AsynchronousPIDController leftDriveControl;
    private AsynchronousPIDController rightDriveControl;

    public CBPWMTankDriveTrain(int lfpin, int rfpin, int gyropin, int LeftChanela, int LeftChanelb, int RightChanela, int RightChanelb) {
        this(new GRTJaguar(lfpin), new GRTJaguar(rfpin), new GRTGyro(
                gyropin, 5, "cangyro"), LeftChanela, LeftChanelb, RightChanela, RightChanelb);
    }

    public CBPWMTankDriveTrain(GRTJaguar lfjaguar, GRTJaguar rfjaguar,
            GRTGyro cangyro, int LeftChanela, int LeftChanelb, int RightChanela, int RightChanelb) {
        this.leftJaguar = lfjaguar;
        leftJaguar.start();

        this.rightJaguar = rfjaguar;
        rightJaguar.start();

        this.leftEncoder = new GRTEncoder(LeftChanela, LeftChanelb, 5, "ljagencoder");

        this.rightEncoder = new GRTEncoder(RightChanela, RightChanelb, 5, "rjagencoder");

        this.canGyro = canGyro;
        leftEncoder.addEncoderListener(this);
        rightEncoder.addEncoderListener(this);
        cangyro.addGyroListener(this);

        canGyro.start();
        leftEncoder.start();
        rightEncoder.start();

        turnControl = new AsynchronousPIDController(new PIDController(turnP,
                turnI, turnD, canGyro, this));
        leftDriveControl = new AsynchronousPIDController(new PIDController(
                driveP, driveI, driveD, leftEncoder, this));
        rightDriveControl = new AsynchronousPIDController(new PIDController(
                driveP, driveI, driveD, rightEncoder, this));
        pointTurn = true;
        leftDriveControl.addPIDListener(this);
        rightDriveControl.addPIDListener(this);
        turnControl.addPIDListener(this);
        leftDriveControl.start();
        rightDriveControl.start();
        turnControl.start();

        leftWheelX = -ROBOT_WIDTH / 2;
        rightWheelX = ROBOT_WIDTH / 2;
        leftWheelY = rightWheelY = 0;

        addActuator("lfJaguar", leftJaguar);
        addActuator("rbJaguar", rightJaguar);
        addSensor("ljagencoder", leftEncoder);
        addSensor("rjagencoder", rightEncoder);
        addSensor("cangyro", canGyro);

    }

    public double getDriveP() {
        return driveP;
    }

    public void setDriveP(double driveP) {
        this.driveP = driveP;
    }

    public double getDriveI() {
        return driveI;
    }

    public void setDriveI(double driveI) {
        this.driveI = driveI;
    }

    public double getDriveD() {
        return driveD;
    }

    public void setDriveD(double driveD) {
        this.driveD = driveD;
    }

    /**
     * Drive dts, specifying speed
     *
     * @param leftSpeed
     * @param rightSpeed
     */
    public void tankDrive(double leftSpeed, double rightSpeed) {
        this.leftJaguar.enqueueCommand(leftSpeed);
        this.rightJaguar.enqueueCommand(rightSpeed);
    }

    public void driveToPosition(double position) {
        driveToPosition(position, position);
    }

    /**
     * Drive dts to a specific position, provided where the l&r wheels should be
     *
     * @param leftPosition
     * @param rightPosition
     */
    public void driveToPosition(double leftPosition, double rightPosition) {
        stopPIDControl();
        leftDriveControl.getPidThread().setSetpoint(leftPosition);
        rightDriveControl.getPidThread().setSetpoint(rightPosition);
        leftDriveControl.enable();
        rightDriveControl.enable();
    }

    public void driveDistance(double distance) {
        driveDistance(distance, distance);
    }

    /**
     * Drive l&r wheels a certain distance
     *
     * @param leftDistance
     * @param rightDistance
     */
    public void driveDistance(double leftDistance, double rightDistance) {
        driveToPosition(leftEncoder.getState("Distance") + leftDistance,
                rightEncoder.getState("Distance") + rightDistance);
    }

    public void turnTo(double angle) {
        turnTo(angle, true);
    }

    /**
     * Turn the robot to a specific heading
     *
     * @param angle
     * @param point
     */
    public void turnTo(double angle, boolean point) {
        stopPIDControl();
        pointTurn = point;
        turnControl.getPidThread().setSetpoint(angle);
        turnControl.enable();
    }

    public void pidWrite(double output) {
        // Scale speed control to output ratio
        if (pointTurn) {
            tankDrive(output, -output);
        } else {
            tankDrive((1 + output) / 2, 1 - (1 + output) / 2);
        }
    }

    /**
     * Stop the driveTrains
     */
    public void stop() {
        stopPIDControl();
        tankDrive(0, 0);
    }

    public double getTurnP() {
        return turnP;
    }

    public void setTurnP(double turnP) {
        this.turnP = turnP;
    }

    public double getTurnI() {
        return turnI;
    }

    public void setTurnI(double turnI) {
        this.turnI = turnI;
    }

    public double getTurnD() {
        return turnD;
    }

    public void setTurnD(double turnD) {
        this.turnD = turnD;
    }

    public boolean isPointTurn() {
        return pointTurn;
    }

    public void setPointTurn(boolean pointTurn) {
        this.pointTurn = pointTurn;
    }

    private void block() {
        try {
            Thread.sleep(WAIT_INTERVAL);
        } catch (InterruptedException e) {
            e.printStackTrace();
        }
    }

    public double getLeftPosition() {
        return leftEncoder.getState("Distance");
    }

    public double getRightPosition() {
        return rightEncoder.getState("Distance");
    }

    public double getHeading() {
        return canGyro.getState("Angle");
    }

    public void changedDirection(EncoderEvent e) {
        // TODO Auto-generated method stub
    }

    public void countDidChange(EncoderEvent e) {
        double deltaR = e.getDistance() - e.getSource().getState("Previous");
        if (e.getSource().getId().equals("ljagencoder")) {
            // left wheel
            leftWheelX = deltaR * Math.cos(Math.toRadians(getHeading()));
            leftWheelY = deltaR * Math.sin(Math.toRadians(getHeading()));
        } else if (e.getSource().getId().equals("rjagencoder")) {
            // right wheel
            rightWheelX = deltaR * Math.cos(Math.toRadians(getHeading()));
            rightWheelY = deltaR * Math.sin(Math.toRadians(getHeading()));
        }
    }

    public void rotationDidStart(EncoderEvent e) {
        // TODO Auto-generated method stub
    }

    public void rotationDidStop(EncoderEvent e) {
        // TODO Auto-generated method stub
    }

    public void didAngleChange(GyroEvent e) {
    }

    public void didAngleSpike(GyroEvent e) {
        // TODO Auto-generated method stub
    }

    public void didReceiveAngle(GyroEvent e) {
        // TODO Auto-generated method stub
    }

    public double getLeftWheelX() {
        return leftWheelX;
    }

    public double getLeftWheelY() {
        return leftWheelY;
    }

    public double getRightWheelX() {
        return rightWheelX;
    }

    public double getRightWheelY() {
        return rightWheelY;
    }

    public double getX() {
        return (getLeftWheelX() + getRightWheelX()) / 2;
    }

    public double getY() {
        return (getRightWheelY() + getRightWheelY()) / 2;
    }

    public void stopPIDControl() {
        leftDriveControl.reset();
        rightDriveControl.reset();
        turnControl.reset();
    }

    public void suspendPIDControl() {
        leftDriveControl.disable();
        rightDriveControl.disable();
        turnControl.disable();
    }

    public void onSetpointReached(PIDEvent e) {
        e.getSource().reset();
    }
}
